#define GENERIC_PROJECT 404
#define MODEL 18F24K22
#include "main.h"

/** C O N F I G U R A T I O N   B I T S ******************************/
#pragma config FOSC = HSHP, FCMEN = OFF, IESO = OFF                            // CONFIG1H
#pragma config PWRTEN = OFF, BOREN = OFF, BORV = 190                            // CONFIG2L
#pragma config WDTEN = OFF, WDTPS = 32768                                   // CONFIG2H
#pragma config MCLRE = EXTMCLR, PBADEN = OFF                      // CONFIG3H
#pragma config STVREN = ON, LVP = OFF, XINST = OFF                          // CONFIG4L
#pragma config CP0 = OFF, CP1 = OFF, CP2 = OFF, CP3 = OFF                   // CONFIG5L
#pragma config CPB = OFF, CPD = OFF                                         // CONFIG5H
#pragma config WRT0 = OFF, WRT1 = OFF, WRT2 = OFF, WRT3 = OFF               // CONFIG6L
#pragma config WRTB = OFF, WRTC = OFF, WRTD = OFF                           // CONFIG6H
#pragma config EBTR0 = OFF, EBTR1 = OFF, EBTR2 = OFF, EBTR3 = OFF           // CONFIG7L
#pragma config EBTRB = OFF                                                  // CONFIG7H

#define HEALTH_WAIT_TIME 2 // -1
#define RING_TAG_LENGTH 16

/***Global Variables******************************************/
char ring_tag[RING_TAG_LENGTH];
int curr_ring = 0;
int wait;
int curr_song = 0;
int check_song = 0;
unsigned int song_wait = 0;
int on = 0;
int r = 0;
int y = 0;
int g = 0;
int health_wait = 0;
int play_time = 0;
char rings = 0, command; // Upper contains valid rings, lower contains current rings
char ring_values[4];
int invalid_ring_present = 0;
char valid_ring = 0;
long int tmr2_cycles = 0;
int read_ring = 0;
int transmit = 0;
char curr_health = 0xFF;

// IR Stuff
char ir_data[48];
char ir_init_data[2];
char ir_start = 0;
char ir_bit = -1;
char ir_data_computed[6];

// Xbee Stuff
char last_out[9];
char data_out_location = 0;
char last_in[9];
char data_in_location = 0;
char *nothing;
char id[2];

void main()
{
	int in = 0, error, i;
	char xbee[27] = {'X', 'B', 'e', 'e', ' ', 'c', 'o', 'm', 'm', 'u', 'n', 'i', 'c', 'a', 't', 'i', 'o', 'n', ' ', 'r', 'e', 'c', 'e', 'i', 'v', 'e', 'd'}, \
	play[12] = {'P', 'l', 'a', 'y', 'i', 'n', 'g', ' ', 's', 'o', 'n', 'g'};
	last_out[0] = 0xFF;
	last_out[1] = 0xFF;
	last_out[2] = 0xFF;
	last_out[3] = 0xFF;
	last_out[4] = 0xFF;
	last_in[data_out_location++] = 0xFF;
	last_in[data_out_location++] = 0xFF;
	last_in[data_out_location++] = 0xFF;
	last_in[data_out_location++] = 0xFF;
	last_in[data_out_location++] = 0xFF;
	*nothing = 0x00;
	for(i = 0; i < 4; i++){ring_values[i] = 0;}
	init();
	// Begin startup sequence
	LATB |= 0x02;
	while (health_wait < HEALTH_WAIT_TIME){if (PIR2bits.TMR3IF){health_wait++;PIR2bits.TMR3IF = 0;}}
	health_wait = 0;
	LATB &= ~0x02;
	LATB |= 0x04;
	while (health_wait < HEALTH_WAIT_TIME){if (PIR2bits.TMR3IF){health_wait++;PIR2bits.TMR3IF = 0;}}
	health_wait = 0;
	LATB &= ~0x04;
	LATB |= 0x08;
	while (health_wait < HEALTH_WAIT_TIME){if (PIR2bits.TMR3IF){health_wait++;PIR2bits.TMR3IF = 0;}}
	health_wait = 0;
	LATB &= ~0x08;
	// End startup sequence
    interrupts_init();

	while(1)
	{
		//Update Health Lights
		if (PIR2bits.TMR3IF)
		{
			if (on)
			{
				if (r) LATB &= ~0x02;
				if (y) LATB &= ~0x04;
				if (g) LATB &= ~0x08;
				on = 0;
			}
			else
			{
				if (r) LATB |= 0x02;
				if (y) LATB |= 0x04;
				if (g) LATB |= 0x08;
				on = 1;
			}
			PIR2bits.TMR3IF = 0;
		}
		//Sound stuff	
		if (PIR1bits.TMR1IF)
		{
			if (PORTA & 0x10)
			{
				if (play_time++ > SONG_PLAY_TIME)
				{
					LATA &= ~0x30;
					play_time = 0;
				}
			}
			PIR1bits.TMR1IF = 0;
		}
		// Need to read ring
		if (read_ring)
		{
			valid_ring = add_ring();
			TXREG1 = valid_ring;
			if (valid_ring)
			{
				//Ring Scanned
				play_song(0);
			}
			LATB &=~0x10;
			tmr2_cycles = 0;
			T2CONbits.TMR2ON = 0;
			read_ring = 0;
		}
		// Server Messages
		if (data_in_location == 0x05 + last_in[2] || data_in_location > 8)
		{
			if (check_packet())
			{
				if (last_in[3] == 'h')
				{
					if (last_in[2] == 0x01)
					{
						if (last_in[4] == 0x00)
						{
							//Dead
							play_song(4);
							change_health(0xFF);
						}
						else
						{
							if ((curr_health < last_in[4] && !((last_in[4] & 0x80) ^ (curr_health & 0x80))) || ((last_in[4] & 0x80) && !(curr_health & 0x80)))
							{
								//Healing Sound
								play_song(7);
							}
							else
							{
								//Damage
								play_song(3);
							}
							change_health((last_in[4] >> 1)/19);
						}
						curr_health = last_in[4];
					}
					else
					{
						create_packet(0x00, nothing, 0x00);
					}
				}
				else if (last_in[3] == 0x00  && last_out[3] != 0x00)
				{
					data_out_location = 0;
				}
			}
			else
			{
				create_packet(0x00, nothing, 0x00);
			}
			data_in_location = 0;
		}
		else if (data_in_location < 0x05 + last_in[2])
		{
			if (PIR1bits.RC1IF)
			{
				last_in[data_in_location++] = RCREG1;
			}
			if (data_in_location == 0x01 && last_in[0] != id[0])
			{
				data_in_location -= 1;
			}
			if (data_in_location == 0x02 && last_in[1] != id[1])
			{
				data_in_location -= 2;
			}
		}
		// Our Messages
		if (data_out_location < 0x05 + last_out[2])
		{
			if (PIR1bits.TX1IF)
			{
				TXREG1 = last_out[data_out_location++];
			}
		}
		// Ring check
		if ((rings) ^ (~PORTA & 0x0F))
		{			
			if (!invalid_ring_present)
			{
				if (((rings & 0x08) && (PORTA & 0x08))) // Ring taken off
				{
					create_packet('r', &ring_values[3], 0x01);ring_values[3] = 0;rings = ~PORTA & 0x0F;LATC &= ~0x20;LATC |= 0x10;
					//Taken off
					play_song(2);
				}
				else if (((rings & 0x04) && (PORTA & 0x04)))
				{
					create_packet('r', &ring_values[2], 0x01);ring_values[2] = 0;rings = ~PORTA & 0x0F;LATC &= ~0x20;LATC |= 0x10;
					//Taken off
					play_song(2);
				}
				else if (((rings & 0x02) && (PORTA & 0x02)))
				{
					create_packet('r', &ring_values[1], 0x01);ring_values[1] = 0;rings = ~PORTA & 0x0F;LATC &= ~0x20;LATC |= 0x10;
					//Taken off
					play_song(2);
				}
				else if (((rings & 0x01) && (PORTA & 0x01)))
				{
					create_packet('r', &ring_values[0], 0x01);ring_values[0] = 0;rings = ~PORTA & 0x0F;LATC &= ~0x20;LATC |= 0x10;
					//Taken off
					play_song(2);
				}
				else									//Ring added on
				{
					if (valid_ring)
					{
						for (i = 0; i < 4; i++)
						{
							if(ring_values[i] == valid_ring)
							{
								valid_ring = 0;
								create_packet('r', &valid_ring, 0x01);
								//Explosion
								play_song(5);
								change_health(0xFF);
								goto invalid;
							}
						}
						if (((~rings & 0x08) && (~PORTA & 0x08))) // Ring taken off
						{
							ring_values[3] = valid_ring;
						}
						else if (((~rings & 0x04) && (~PORTA & 0x04)))
						{
							ring_values[2] = valid_ring;
						}
						else if (((~rings & 0x02) && (~PORTA & 0x02)))
						{
							ring_values[1] = valid_ring;
						}
						else if (((~rings & 0x01) && (~PORTA & 0x01)))
						{
							ring_values[0] = valid_ring;
						}
						create_packet('r', &valid_ring, 0x01);
						rings = (~PORTA & 0x0F);
						valid_ring = 0;
						LATC &= ~0x20;
						LATC |= 0x10;
						//Put on
						play_song(1);
					}
					else
					{
						if (!invalid_ring_present)
						{
							//Invalid Ring on
							play_song(6);
						}
invalid:				invalid_ring_present = 1;
						LATC &= ~0x10;
						LATC |= 0x20;
					}
				}
				LATC &= ~0x0F;
				LATC |= rings & 0x0F;
			}
		}
		else
		{
			if (invalid_ring_present)
			{	
				LATC &= ~0x20;
				LATC |= 0x10;
			}
			invalid_ring_present = 0;
		}

/*		if (PIR1bits.RC1IF)
		{
			if (RCSTA1bits.OERR)
			{
				command = RCREG1;
				command = RCREG1;
				RCSTAbits.CREN = 0;
				RCSTAbits.CREN = 1;
			}
			command = RCREG1;
			switch (command)
			{
				case 'a':play_time = 0;play_song(0);break;
				case 'b':play_time = 0;play_song(1);break;
				case 'c':play_time = 0;play_song(2);break;
				case 'd':play_time = 0;play_song(3);break;
				case 'e':play_time = 0;play_song(4);break;
				case 'f':play_time = 0;play_song(5);break;
				case 'g':play_time = 0;play_song(6);break;
				case 'h':change_health();break;
				case 'r':valid_ring = add_ring();break;
				case 'v':while (!PIR1bits.TX1IF);TXREG1 = valid_ring;break;
				case '1':while (!PIR1bits.TX1IF);TXREG1 = rings;break;
				case '2':while (!PIR1bits.TX1IF);TXREG1 = PORTA;break;
				case '3':while (!PIR1bits.TX1IF);TXREG1 = PORTC;break;
				case '4':while (!PIR1bits.TX1IF);TXREG1 = ring_values[0];break;
				case '5':while (!PIR1bits.TX1IF);TXREG1 = ring_values[1];break;
				case '6':while (!PIR1bits.TX1IF);TXREG1 = ring_values[2];break;
				case '7':while (!PIR1bits.TX1IF);TXREG1 = ring_values[3];break;
				case 'x':while (msg_loc < 27){if (PIR1bits.TX1IF){TXREG1 = xbee[msg_loc++];}}msg_loc = 0;
			}
		}*/
	}	
}

void init()
{
	ANSELA &= 0x00;
	ANSELB &= 0x00;
	ANSELC &= 0x00;
	main_init();
	rfid_init();
	audio_init();
	xbee_init();
}

void main_init()
{
	int i;
	//Timer3 Init
	T3CONbits.TMR3ON = 1; 		// Starts timer1
	T3CONbits.T3CKPS0 = 1;		// Setting the predivide/prescale for timer1 to 1:8
	T3CONbits.T3CKPS1 = 1;		// Setting the predivide/prescale for timer1 to 1:8
	//Health LEDs
	TRISB &= ~0x0E;
	LATB &= ~0x0E;
	//Ring LEDs
	TRISC &= ~0x0F;
	LATC &= ~0x0F;
	//Test LEDs
	TRISC &= ~0x30;
	LATC &= ~0x30;
	//Timer4
	T4CON = 0b01001100;
	IPR5bits.TMR4IP = 1;		//High Interrupt
	PIE5bits.TMR4IE = 1;
	PR4 = 0xFA;
	//RB0 INT
	INTCON2bits.INTEDG0 = 0;
	INTCONbits.INT0IE = 1;
	for(i=0; i < 25; i++){ir_data[i] = 0;}
}

void change_health(char val)
{
	switch (val)
	{
		case 0x06:LATB &= ~0x0E;LATB |= 0x08;g = 0; y = 0; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		case 0x05:LATB &= ~0x0E;g = 1; y = 0; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		case 0x04:LATB &= ~0x0E;LATB |= 0x0C;g = 0; y = 0; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		case 0x03:LATB &= ~0x0E;LATB |= 0x04;g = 0; y = 0; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		case 0x02:LATB &= ~0x0E;g = 0; y = 1; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		case 0x01:LATB &= ~0x0E;LATB |= 0x06;g = 0; y = 0; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		case 0x00:LATB &= ~0x0E;g = 0; y = 0; r = 1;LATC &= ~0x20;LATC |= 0x10;break;
		case 0xFF:LATB &= ~0x0E;LATB |= 0x02;g = 0; y = 0; r = 0;LATC &= ~0x20;LATC |= 0x10;break;
		default:LATC &= ~0x10;LATC |= 0x20;
	}
}

char add_ring()
{
	int i = 0;
	LATC &= ~0x20;
	LATC |= 0x10;
	while (i == 0)
	{
		if (PIR3bits.RC2IF)
		{
			if (RCSTA2bits.OERR)
			{
				ring_tag[0] = RCREG2;
				ring_tag[0] = RCREG2;
				RCSTA2bits.CREN = 0;
				RCSTA2bits.CREN = 1;
			}
			ring_tag[0] = RCREG2;
			if (ring_tag[0] == 0x02)
			{
				i++;
			}
		}
		//Return false if we are no longer powering the RFID reader
		if (!(PORTB & 0x10))
		{
			return 0;
		}
	}
	while (i < RING_TAG_LENGTH)
	{
		if (PIR3bits.RC2IF)
		{
			ring_tag[i++] = RCREG2;
		}
		//Return false if we are no longer powering the RFID reader
		if (!(PORTB & 0x10))
		{
			return 0;
		}
	}
	if (ring_tag[13] == 0x0D && ring_tag[14] == 0x0A && ring_tag[15] == 0x03)
	{
		//BLUE RING
		if (ring_tag[1] == '4' && ring_tag[2] == 'D' && ring_tag[3] == '0' && ring_tag[4] == '0' && ring_tag[5] == '6' && ring_tag[6] == 'A' && ring_tag[7] == 'A' && ring_tag[8] == 'A' && ring_tag[9] == 'A' && ring_tag[10] == '3' && ring_tag[11] == '2' && ring_tag[12] == 'E')
		{
			LATC &= ~0x20;
			LATC |= 0x10;
			return 1;
		}
		//GREEN RING
		else if (ring_tag[1] == '4' && ring_tag[2] == 'D' && ring_tag[3] == '0' && ring_tag[4] == '0' && ring_tag[5] == '6' && ring_tag[6] == 'A' && ring_tag[7] == 'C' && ring_tag[8] == 'D' && ring_tag[9] == '3' && ring_tag[10] == 'E' && ring_tag[11] == 'D' && ring_tag[12] == '4')
		{
			LATC &= ~0x20;
			LATC |= 0x10;
			return 2;
		}
		//CLEAR RING
		else if (ring_tag[1] == '4' && ring_tag[2] == 'D' && ring_tag[3] == '0' && ring_tag[4] == '0' && ring_tag[5] == '6' && ring_tag[6] == 'A' && ring_tag[7] == 'B' && ring_tag[8] == 'C' && ring_tag[9] == '8' && ring_tag[10] == '5' && ring_tag[11] == '1' && ring_tag[12] == 'E')
		{
			LATC &= ~0x20;
			LATC |= 0x10;
			return 3;
		}
	}
	LATC &= ~0x10;
	LATC |= 0x20;
	return 0;
}

int ir_decode()
{
	int i, valid = 1;
	for (i=0; i < 6; i++){ir_data_computed[i] = 0;}
	for (i=0; i < 48; i++)
	{
		ir_data_computed[i/8] |= (ir_data[i] > 2 ? 1 : 0) << (7 - (i % 8));
	}
	for(i = 0; i < 3; i++)
	{
		if (ir_data_computed[i] != ~ir_data_computed[i+3]) valid = 0;
	}
	if (valid)
	{
		LATC |= 0x10;
		return 1;
	}
	return 0;
}

void create_packet(char code, char *data, char length)
{
	char *data_location = last_out;
	char checksum = 0;
	char i;
	*data_location++ = id[0];
	checksum += id[0];
	*data_location++ = id[1];
	checksum += id[1];
	*data_location++ = length;
	checksum += length;
	*data_location++ = code;
	checksum += code;
	for (i = 0; i < length; i++)
	{
		checksum += *data;
		*data_location++ = *data++;
	}
	*data_location++ = checksum;
	
	data_out_location = 0x00;
}

int check_packet()
{
	char checksum = 0, i;
	for (i = 0; i < (4 + last_in[2]); i++)
	{
		checksum += last_in[i];
	}
	if (checksum == last_in[4 + last_in[2]])
	{
		return 1;
	}
	else
	{
		return 0;
	}
}

void xbee_init()
{
	int n;
	int point = 0;
	int fail = 0;							// RX flag
	int TimeOutValue = 10;  				// Number of checks before timeout of TX Check
	char txTest[8] = "TX Test";
	char rxTest[8] = "RX Test";
	char blank = '\0';
	//UART1 Init
	TXSTA1bits.BRGH = 1;
	SPBRG1 = 0x81;				// Baud number 129
	TRISC |= 0xC0;
	RCSTA1bits.SPEN = 1;
	TXSTA1bits.SYNC = 0;
	TXSTA1bits.TXEN = 1;
	RCSTA1bits.CREN = 1;
	while(fail < 7)
	{
//		xbee_reset();
		strToXbee(txTest);
		for(n = 0; n < TimeOutValue; n++)
		{
			fail = 0;					// Set RX Fail to zero
			point = 0;
			while(point < 7)
			{
				while(!PIR1bits.RC1IF)
				{
					if (RCSTA1bits.OERR)
					{
						blank = RCREG1;
						blank = RCREG1;
						RCSTA1bits.CREN = 0;
						RCSTA1bits.CREN = 1;
					}
				}
				if(RCREG1 == rxTest[point++])
				{
					fail++;
				}
				else
				{
					break;
				}
			}
			if(fail == 7)
			{
				break;
			}
		}
	}
	while(!PIR1bits.RC1IF);
	id[0] = RCREG1;
	while(!PIR1bits.RC1IF);
	id[1] = RCREG1;
	while(!PIR1bits.TX1IF);
	TXREG1 = id[0];
	while(!PIR1bits.TX1IF);
	TXREG1 = id[1];
}

void strToXbee(char *TXData)
{
	while(*TXData != '\0')
	{
		if (PIR1bits.TX1IF)
		{
			TXREG1 = *(TXData++);
		}
	}
}

//----------------------------------------------------------------------------
//INTERRUPTS

void interrupts_init()
{
    RCONbits.IPEN = 1;          // Enable priority levels on interrupts
	INTCONbits.GIEH = 1;        // Interrupt high enabled.
	INTCONbits.GIEL = 1;		// Interrupt low enabled.
}

//----------------------------------------------------------------------------
// High priority interrupt vector
#pragma code InterruptVectorHigh = 0x08
void InterruptVectorHigh (void)
{
  _asm
     goto InterruptServiceHigh //jump to interrupt routine
  _endasm
}

//----------------------------------------------------------------------------
// Low priority interrupt vector
#pragma code InterruptVectorLow = 0x18
void InterruptVectorLow (void)
{
  _asm
     goto InterruptServiceLow //jump to interrupt routine
  _endasm
}


// -------------------- Iterrupt Service Routines --------------------------
#pragma interrupt InterruptServiceHigh  // "interrupt" pragma also for high priority
void InterruptServiceHigh()
{
	int i;
	if(PIR5bits.TMR4IF)
	{
		if(ir_start)
		{
			if (ir_init_data[0] == ir_init_data[1] && ir_init_data[1] == 0x08)
			{
				if (PORTB & 0x01)
				{
					ir_data[ir_bit]++;
				}
			}
			else
			{
				if (~PORTB & 0x01)
				{
					if (ir_init_data[1] == 0)
					{
						ir_init_data[0]++;
					}
					else
					{
						ir_start = 0;
						ir_bit = -1;
						for(i=0; i < 48; i++){ir_data[i] = 0;}
						ir_init_data[0] = 0;
						ir_init_data[1] = 0;
					}
				}
				else
				{
					ir_init_data[1]++;
					if (ir_init_data[0] != 8)
					{

						ir_start = 0;
						ir_bit = -1;
						for(i=0; i < 48; i++){ir_data[i] = 0;}
						ir_init_data[0] = 0;
						ir_init_data[1] = 0;
					}
				}
			}
		}
		PIR5bits.TMR4IF = 0;
	}
	else if(INTCONbits.INT0IF)
	{
		if(ir_start)
		{
			ir_bit++;
			if(ir_bit == 48)
			{
				ir_start = 0;
				if (ir_decode())
				{
					create_packet('i', (char *)&ir_data_computed, 0x03); 
				}
				ir_bit = -1;
				for(i=0; i < 48; i++){ir_data[i] = 0;}
				ir_init_data[0] = 0;
				ir_init_data[1] = 0;
			}
		}
		else
		{
			ir_start = 1;
		}
		INTCONbits.INT0IF = 0;
	}
}  // return from high-priority interrupt

#pragma interrupt InterruptServiceLow
void InterruptServiceLow()
{
	// Turn on the RFID Reader
	if (INTCONbits.RBIF)
	{
		if (!(PORTB & 0x20)) {
			LATB |= 0x10;
			T2CONbits.TMR2ON = 1;
			read_ring = 1;
		}
		INTCONbits.RBIF = 0;		
	}
	else if (PIR1bits.TMR2IF) {
		if (tmr2_cycles++ > 5000) {
			LATB &=~0x10;
			tmr2_cycles = 0;
			T2CONbits.TMR2ON = 0;
			read_ring = 0;
		}
		PIR1bits.TMR2IF = 0;
	}
}
